
from maya import cmds as cmds
import math , random
import FCVector

from FCGenericCrowdEngine import Define , Util, Terrain, Model, FCSteering

reload(Define)
reload(FCVector)
reload(Util)
reload(Terrain)
reload(Model)
reload(FCSteering)

class Feeler:
    mFeelerList = []
    
    def __init__( self, _Position, _Heading, _WallDetectionLength ):
        _TempVector = _Position + ( _Heading * _WallDetectionLength )
        self.mFeelerList.append( _TempVector )

        _HalfPie = 3.1415 / 2.0
        
        _Temp = FCVector.vector2D( _Heading.x, _Heading.y )
        FCVector.rotate( _Temp, _HalfPie * 3.5 )
        self.mFeelerList.append( _Position + ( _Temp * (_WallDetectionLength / 2.0) ) )            	

        _Temp = FCVector.vector2D( _Heading.x, _Heading.y )
        FCVector.rotate(_Temp, _HalfPie * 0.5 )
        self.mFeelerList.append( _Position + ( _Temp * (_WallDetectionLength / 2.0) ) )
        
    def getFeeler( self, _Idx ):
        return self.mFeelerList[_Idx]
        
    def fnUpdate( self, _Position, _Heading, _WallDetectionLength ):
        _TempVector = _Position + ( _Heading * _WallDetectionLength )
        self.mFeelerList[0] = _TempVector
    
        _HalfPie = 3.1415 / 2.0
        _Temp = FCVector.vector2D( _Heading.x, _Heading.y )
        FCVector.rotate( _Temp, _HalfPie * 3.5 )
        self.mFeelerList[1] = _Position + ( _Temp * (_WallDetectionLength / 2.0) )

        _Temp = FCVector.vector2D( _Heading.x, _Heading.y )
        FCVector.rotate(_Temp, _HalfPie * 0.5 )
        self.mFeelerList[2] = _Position + ( _Temp * (_WallDetectionLength / 2.0) )


#-------------------------
# agent state list
# wander, goal
class Agent:
    def __init__( self, _Simulator, _Position, _Radius, _isLod, _quality ):
        self.mIdx = 0
        self.model = Model.Model( _isLod, _quality )
        self.mState = 'wander'
        self.mSteering = FCSteering.FCSteering(self);        
        self.mPosition = _Position
        self.mHeading = FCVector.vector2D(-1,0)
        self.mSide = FCVector.vector2D(0,0)
        self.mAngle = 0.0
        self.mRadius = _Radius
        
        self.mMaxSpeed = 30.0 * random.uniform( 0.8, 1.2 )
        self.mVelocity = FCVector.vector2D(random.uniform( -1, 1 ), random.uniform( -1, 1 ))
        self.mVelocity *= self.mMaxSpeed
        
        self.mNewVelocity = FCVector.vector2D(0,0)
        self.mPrefVelocity = FCVector.normalize( self.mVelocity ) * self.mMaxSpeed
        
        self.steering = FCVector.normalize( self.mVelocity ) * self.mMaxSpeed
        self.wanderSide = 0
        self.wanderUp = 0
        self.wanderRate = random.uniform( 0.0, 0.2 )
        
        self.mGoal = FCVector.vector2D(0, 0)
        self.mHasGoal = False
        
        self.mMaxNeighbor = 100
        self.mNeighborDist = _Radius * 1.5
        self.mTimeHorz = 2.0
        self.mTimeHorzObst = 2.0
        
        self.mSpendTime = 0.0
        self.mChangeTime = 0.0
        
        self.mSimulator = _Simulator
        self.mAgentList = []
        self.mObstacleList = []
        self.mOrcaLines = []
        
        self.mMass = 1.0
        self.mMaxforce = 200
        self.mRandomscale = 200
        self.mWalldetectionfeelerLength = 800
        self.mWallavoidWeight = 100
        self.mRandomWeight = 0.5
        
        self.mTag = False
        
        #self.mFeelers = Feeler( self.mPosition, self.mHeading, self.mWalldetectionfeelerLength )
    def Tag(self):
        self.mTag = True
    def UnTag(self):
        self.mTag = False        
    def speed():
        return mVelocity.length()
    def setTarget(self,targetPos):
        self.mSteering.SetTarget(targetPos)
    def setGoal( self, x, y ):
        self.mState = 'goal'
        self.mHasGoal = True
        self.mGoal.x = x
        self.mGoal.y = y
        
        self.mVelocity = FCVector.normalize( self.mGoal - self.mPosition )
        self.mVelocity *= self.mMaxSpeed

    def preferredVelocity( self ):
        if self.mState == 'goal':
            self.mPrefVelocity = FCVector.normalize( self.mGoal - self.mPosition ) * self.mMaxSpeed
        else:
            self.mPrefVelocity = self.mVelocity

    def fnUpdate( self, _framelate, _frameCount, _terrainCheck ):
        self.mVelocity = self.mNewVelocity
        
        #self.wander( _framelate )
        self.TagNeighbors(self)
        _steeringforce = self.mSteering.Calculate() 
        _acceleration = _steeringforce / self.mMass
        self.mVelocity += _acceleration * _framelate
        self.mAngle = self.mVelocity.angle( FCVector.UNIT_Y )
        self.mPosition += (self.mVelocity * _framelate)
        
        _tx = self.mPosition.x
        _tz = self.mPosition.y
        _ry = math.degrees( self.mAngle )
        _head = FCVector.normalize( self.mVelocity )

        if _terrainCheck: 
            _result = Terrain.Terrain.instance.fnGetXZRotateAndHeight( _tx, 10000, _tz, _head)    
        else:
            _result = Terrain.Terrain.instance.fnGetInfo( _tx, _tz, _head )

        _rx = math.degrees( _result[0] )
        #_ry = math.degrees( _result[1] )
        _rz = math.degrees( _result[2] )
        _ty = _result[3]

        self.model.update( _tx, _ty, _tz, _rx, _ry, _rz, _frameCount )
        #self.model.updateWalk( _frameCount, self.mVelocity.length() )
        
    def wander( self, _dt ):
        _speed = 100 * _dt
        self.wanderSide = Util.scalarRandomWalk( self.wanderSide, _speed, -1, 1 )
        self.wanderUp = Util.scalarRandomWalk( self.wanderUp, _speed, -1, 1 )
        
        _up = FCVector.normalize( self.mVelocity )
        _side = _up.perp()
        self.mVelocity += ((_side * self.wanderSide) + (_up * self.wanderUp)) * self.mMaxSpeed * self.wanderRate
        
    def fnUpdateFeeler( self, _DetectionLength, head = None ):
        if head == None:
            self.mFeelers.fnUpdate( self.mPosition, self.mHeading, _DetectionLength )
        else:
            self.mFeelers.fnUpdate( self.mPosition, head, _DetectionLength )
    def TagNeighbors(self,entity):
        for _curentity in self.mAgentList:
            _curentity[1].UnTag()
            _to = _curentity[1].mPosition - entity.mPosition
            _range = self.mRadius + _curentity[1].mRadius
            if _curentity[1] != entity and _to.lengthsq()<_range*_range:
                _curentity[1].Tag()
                
                
    def wallavoidance(self,pos,head = None):
        if head==None:
            self.fnUpdateFeeler(self.mWalldetectionfeelerLength)
        else:
            self.fnUpdateFeeler(self.mWalldetectionfeelerLength,head)
        DisToThisIP = [0.0]
        DisToClosestIP = 99999999
        closetWall = -1
        OverShoot =FCVector.vector2D(0,0)    	
        steeringforce = FCVector.vector2D(0,0)
        point = [FCVector.vector2D(0,0)]
        ClosetPoint = FCVector.vector2D(0,0)        
        _Wall = self.mSimulator.mWall

        for flr in range(3):	    	        
            for wcount in range(len(_Wall)):
                if(Util.LineIntersection2D(pos, self.mFeelers.getFeeler(flr), _Wall[wcount].TO(), _Wall[wcount].FROM(), DisToThisIP, point)):
                    if(DisToThisIP[0]<DisToClosestIP):
                        DisToClosestIP = DisToThisIP[0]
                        closetWall = wcount
                        ClosetPoint = point[0]

            if(closetWall>=0):
                OverShoot = self.mFeelers.getFeeler(flr) - ClosetPoint
                steeringforce = _Wall[closetWall].normal()*OverShoot.length()

        return steeringforce
        
    def fnInsertAgent( self, _Agent, _RangeSq ):
        if self.mIdx != _Agent.mIdx:
            _DistSq = (self.mPosition - _Agent.mPosition).lengthsq()
            
            if _DistSq < _RangeSq:                
                if len(self.mAgentList) < self.mMaxNeighbor:
                    self.mAgentList.append( [_DistSq, _Agent] )

                _Idx = len(self.mAgentList) - 1
                while (0 != _Idx) and (_DistSq < self.mAgentList[_Idx-1][0]):
                    self.mAgentList[_Idx] = self.mAgentList[_Idx-1]
                    _Idx -= 1
                    
                self.mAgentList[_Idx] = [_DistSq, _Agent]
                if len(self.mAgentList) == self.mMaxNeighbor:
                    _RangeSq = self.mAgentList[len(self.mAgentList) - 1][0]
                    
        return _RangeSq
    
    def fnInsertObstacle( self, _Obstacle, _RangeSq ):
        if None == _Obstacle or 0 == _Obstacle:
            return
            
        _NextObstacle = _Obstacle.mNextObstacle
        _DistSq = transform.distSqPointLineSegment( _Obstacle.mPosition, _NextObstacle.mPosition, self.mPosition )
        if _DistSq < _RangeSq:
            self.mObstacleList.append( [_DistSq, _Obstacle] )
            
            _Idx = len(self.mObstacleList) - 1
            while (0 != _Idx and _DistSq < self.mObstacleList[_Idx-1][0]):
                self.mObstacleList[_Idx] = self.mObstacleList[_Idx-1]
                _Idx -= 1
            
            self.mObstacleList[_Idx] = [_DistSq, _Obstacle]

    def fnComputeNeighbors( self ):
        self.mObstacleList = []

        _RangeSq = Util.square( self.mTimeHorzObst * self.mMaxSpeed + self.mRadius )
        self.mSimulator.mKdTree.fnComputeObstacleNeighbors( self, _RangeSq )

        self.mAgentList = []
        if 0 < self.mMaxNeighbor:
            _RangeSq = Util.square( self.mNeighborDist )
            _RangeSq = self.mSimulator.mKdTree.fnComputeAgentNeighbors( self, _RangeSq )

    def fnComputeNewVelocity( self ):
        self.mOrcaLines = []

        _InvTimeHorzObst = 1.0 / self.mTimeHorzObst

        for _ObstIdx in xrange( 0, len(self.mObstacleList) ):
            _pObstacle1 = self.mObstacleList[_ObstIdx][1]
            _pObstacle2 = _pObstacle1.mNextObstacle

            _RelativePosition1 = _pObstacle1.mPosition - self.mPosition
            _RelativePosition2 = _pObstacle2.mPosition - self.mPosition

            _AlreadyCovered = False

            for _LineIdx in xrange( 0, len(self.mOrcaLines) ):
                _Det1 = transform.determinant( _InvTimeHorzObst * _RelativePosition1 - self.mOrcaLines[_LineIdx].Point, self.mOrcaLines[_LineIdx].Direction )
                _Det2 = transform.determinant( _InvTimeHorzObst * _RelativePosition2 - self.mOrcaLines[_LineIdx].Point, self.mOrcaLines[_LineIdx].Direction )

                if (_Det1 - _InvTimeHorzObst * self.mRadius >= -transform.RVO_EPSILON) and \
                    (_Det2 - _InvTimeHorzObst * self.mRadius >= -transform.RVO_EPSILON):
                    _AlreadyCovered = True
                    break

            if _AlreadyCovered:
                continue

            _DistSq1 = _RelativePosition1.lengthsq()
            _DistSq2 = _RelativePosition2.lengthsq()
            _RadiusSq = Util.square( self.mRadius )

            _ObstacleVec = _pObstacle1.mPosition - _pObstacle2.mPosition
            _S = (-_RelativePosition1.dot( _ObstacleVec ) ) / _ObstacleVec.lengthsq()
            _DistSqLine = (-_RelativePosition1 - _S * _ObstacleVec).lengthsq()

            _Line = Define.Line()
            if (0.0 > _S) and (_DistSq1 <= _RadiusSq):
                if _pObstacle1.mIsConvex:
                    _Line.Point = FCVector.vector2D( 0.0, 0.0 )
                    _Line.Direction = FCVector.vector2D(-_RelativePosition1.y, _RelativePosition1.x)
                    _Line.Direction.normalize()
                    self.mOrcaLines.append( _Line )
                continue

            elif (1.0 < _S) and (_DistSq2 <= _RadiusSq):
                if _pObstacle2.mIsConvex and (0.0 <= transform.determinant(_RelativePosition2, _pObstacle2.mNormal)):
                    _Line.Point = FCVector.vector2D( 0, 0 )
                    _Line.Direction = FCVector.vector2D(-_RelativePosition2.y, _RelativePosition2.x)
                    _Line.Direction.normalize()
                    self.mOrcaLines.append( _Line )
                continue

            elif (0.0 <= _S and 1.0 > _S) and (_DistSqLine <= _RadiusSq):
                _Line.Point = FCVector.vector2D( 0, 0 )
                _Line.Direction = -_pObstacle1.mNormal
                self.mOrcaLines.append( _Line )
                continue

            _LeftLegDir = FCVector.vector2D( 0.0, 0.0 )
            _RightLegDir = FCVector.vector2D( 0.0, 0.0 )

            if (0.0 > _S) and (_DistSqLine <= _RadiusSq):
                if not _pObstacle1.mIsConvex:
                    continue

                _pObstacle2 = _pObstacle1

                _Leg1 = math.sqrt( math.fabs(_DistSq1 - _RadiusSq) )
                _LeftLegDir = FCVector.vector2D( (_RelativePosition1.x * _Leg1) - (_RelativePosition1.y * self.mRadius), (_RelativePosition1.x * self.mRadius) + (_RelativePosition1.y * _Leg1) ) / _DistSq1
                _RightLegDir = FCVector.vector2D( (_RelativePosition1.x * _Leg1) + (_RelativePosition1.y * self.mRadius), (-_RelativePosition1.x * self.mRadius) + (_RelativePosition1.y * _Leg1) ) / _DistSq1

            elif (1.0 < _S) and (_DistSqLine <= _RadiusSq):
                if not _pObstacle2.mIsConvex:
                    continue

                _pObstacle1 = _pObstacle2;

                _Leg2 = math.sqrt( _DistSq2 - _RadiusSq )
                _LeftLegDir = FCVector.vector2D( (_RelativePosition2.x * _Leg2) - (_RelativePosition2.y * self.mRadius), (_RelativePosition2.x * self.mRadius) + (_RelativePosition2.y * _Leg2) ) / _DistSq2
                _RightLegDir = FCVector.vector2D( (_RelativePosition2.x * _Leg2) + (_RelativePosition2.y * self.mRadius), (-_RelativePosition2.x * self.mRadius) + (_RelativePosition2.y * _Leg2) ) / _DistSq2

            else:
                if _pObstacle1.mIsConvex:
                    _Leg1 = math.sqrt( _DistSq1 - _RadiusSq )
                    _LeftLegDir = FCVector.vector2D(_RelativePosition1.x * _Leg1 - _RelativePosition1.y * self.mRadius, _RelativePosition1.x * self.mRadius + _RelativePosition1.y * _Leg1 ) / _DistSq1
                else:
                    _LeftLegDir = -_pObstacle1.mNormal

                if _pObstacle2.mIsConvex:
                    _Leg2 = math.sqrt( _DistSq2 - _RadiusSq )
                    _RightLegDir = FCVector.vector2D(_RelativePosition2.x * _Leg2 + _RelativePosition2.y * self.mRadius, -_RelativePosition2.x * self.mRadius + _RelativePosition2.y * _Leg2 ) / _DistSq2
                else:
                    _RightLegDir = _pObstacle2.mNormal

            _LeftNeighbor = _pObstacle1.mPrevObstacle
            _IsLeftLegForeign = False
            _IsRightLegForeign = False

            if _pObstacle1.mIsConvex and (0.0 <= transform.determinant( _LeftLegDir, -_LeftNeighbor.mNormal )):
                _LeftLegDir = -_LeftNeighbor.mNormal
                _IsLeftLegForeign = True

            if _pObstacle2.mIsConvex and (0.0 >= transform.determinant( _RightLegDir, _pObstacle2.mNormal )):
                _RightLegDir = _pObstacle2.mNormal
                _IsRightLegForeign = True

            _LeftCutoff = _InvTimeHorzObst * ( _pObstacle1.mPosition - self.mPosition )
            _RightCutoff = _InvTimeHorzObst * ( _pObstacle2.mPosition - self.mPosition )
            _CutoffVec = _RightCutoff - _LeftCutoff
            _t = 0.5
            if (_pObstacle1 != _pObstacle2): _t = (self.mVelocity - _LeftCutoff).dot( _CutoffVec ) / _CutoffVec.lengthsq()
            
            _tLeft = (self.mVelocity - _LeftCutoff).dot( _LeftLegDir )
            _tRight = (self.mVelocity - _RightCutoff).dot( _RightLegDir )

            if ( (_t < 0.0) and (_tLeft < 0.0) ) or ( (_pObstacle1 == _pObstacle2) and (_tLeft < 0.0) and (_tRight < 0.0) ):
                _NormalW = transform.normalize(self.mVelocity - _LeftCutoff)

                _Line.Direction = FCVector.vector2D(_NormalW.y, -_NormalW.x)
                _Line.Point = _LeftCutoff + (self.mRadius * _InvTimeHorzObst * _NormalW)

                self.mOrcaLines.append( _Line )
                continue

            elif (_t > 1.0) and (_tRight < 0.0):
                _NormalW = transform.normalize(self.mVelocity - _RightCutoff)

                _Line.Direction = FCVector.vector2D(_NormalW.y, -_NormalW.x)
                _Line.Point = _RightCutoff + self.mRadius * _InvTimeHorzObst * _NormalW

                self.mOrcaLines.append( _Line )
                continue

            _DistSqCutoff = ( self.mVelocity - (_LeftCutoff + _t * _CutoffVec) ).lengthsq()
            _DistSqLeft = ( self.mVelocity - (_LeftCutoff + _tLeft * _LeftLegDir ) ).lengthsq()
            _DistSqRight = ( self.mVelocity - (_RightCutoff + _tRight * _RightLegDir ) ).lengthsq()

            if (_t < 0.0) or (_t > 1.0) or (_pObstacle1 == _pObstacle2):
                _DistSqCutoff = float('inf')
            if _tLeft < 0.0:
                _DistSqLeft = float('inf')
            if _tRight < 0.0:
                _DistSqRight = float('inf')

            if (_DistSqCutoff <= _DistSqLeft) and (_DistSqCutoff <= _DistSqRight):
                _Line.Direction = -_pObstacle1.mNormal;
                _Line.Point = _LeftCutoff + self.mRadius * _InvTimeHorzObst * FCVector.vector2D(-_Line.Direction.y, _Line.Direction.x)
                self.mOrcaLines.append( _Line )
                continue
                
            elif _DistSqLeft <= _DistSqRight:
                if _IsLeftLegForeign:
                    continue

                _Line.Direction = _LeftLegDir
                _Line.Point = _LeftCutoff + self.mRadius * _InvTimeHorzObst * FCVector.vector2D(-_Line.Direction.y, _Line.Direction.x)
                self.mOrcaLines.append( _Line )
                continue
            else:
                if _IsRightLegForeign:
                    continue

                _Line.Direction = -_RightLegDir
                _Line.Point = _RightCutoff + self.mRadius * _InvTimeHorzObst * FCVector.vector2D(-_Line.Direction.y, _Line.Direction.x)
                self.mOrcaLines.append( _Line )
                continue


        _NumObstLines = len(self.mOrcaLines)
        _InvTimeHorz = 1.0 / self.mTimeHorz

        for agent in self.mAgentList:
            _pOther = agent[1]

            _RelativePosition = _pOther.mPosition - self.mPosition
            _RelativeVelocity = self.mVelocity - _pOther.mVelocity
            _DistSq = _RelativePosition.lengthsq()
            _CombinedRadius = self.mRadius + _pOther.mRadius
            _CombinedRadiusSq = Util.square(_CombinedRadius)

            _Line = Define.Line()
            _U = FCVector.vector2D(0,0)

            if _DistSq > _CombinedRadiusSq:
                _W = _RelativeVelocity -  _RelativePosition * _InvTimeHorz
                _DotProduct1 = _W.dot( _RelativePosition )

                if (_DotProduct1 < 0.0) and (Util.square(_DotProduct1) > _CombinedRadiusSq * _W.lengthsq()):
                    _NormalW = FCVector.normalize(_W)
                    _Line.Direction = FCVector.vector2D( _NormalW.y, -_NormalW.x )
                    _U = ( _CombinedRadius * _InvTimeHorz - _W.length() ) * _NormalW
                else:
                    _Leg = math.sqrt( _DistSq - _CombinedRadiusSq )

                    if 0.0 < Util.determinant( _RelativePosition, _W ):
                        _Line.Direction = FCVector.vector2D( _RelativePosition.x * _Leg - _RelativePosition.y * _CombinedRadius, _RelativePosition.x * _CombinedRadius + _RelativePosition.y * _Leg ) / _DistSq
                    else:
                        _Line.Direction = -FCVector.vector2D( _RelativePosition.x * _Leg + _RelativePosition.y * _CombinedRadius, -_RelativePosition.x * _CombinedRadius + _RelativePosition.y * _Leg ) / _DistSq

                    _U = _RelativeVelocity.dot( _Line.Direction ) * _Line.Direction - _RelativeVelocity
            else:
                _InvTimeStep = 1.0 / self.mSimulator.mTimeStep

                _W = _RelativeVelocity - _RelativePosition * _InvTimeStep
                _NormalW = FCVector.normalize(_W)

                _Line.Direction = FCVector.vector2D( _NormalW.y, -_NormalW.x )
                _U =  _NormalW * ( _CombinedRadius * _InvTimeStep - _W.length() )
                
            _Line.Point = self.mVelocity + (_U * 0.5)
            self.mOrcaLines.append( _Line )

        _LineFail = LinearProgram2( self.mOrcaLines, self.mMaxSpeed, self.mPrefVelocity, False, self.mNewVelocity )
        if _LineFail < len(self.mOrcaLines):
            LinearProgram3( self.mOrcaLines, _NumObstLines, _LineFail, self.mMaxSpeed, self.mNewVelocity )


def LinearProgram1( _Lines, _LineNum, _Radius, _OptVelocity, _DirectionOpt, _Result ):
    _DotProduct = _Lines[_LineNum].Point.dot( _Lines[_LineNum].Direction )
    _Discriminant = Util.square( _DotProduct ) + Util.square( _Radius ) - _Lines[_LineNum].Point.lengthsq()
    
    if 0.0 > _Discriminant:
	    return False
	    
    _DiscriminantSq = math.sqrt( _Discriminant )
    _tLeft = -_DotProduct - _DiscriminantSq
    _tRight = -_DotProduct + _DiscriminantSq

    for _Idx in xrange( _LineNum ):
        _Denominator = Util.determinant( _Lines[_LineNum].Direction, _Lines[_Idx].Direction )
        _Numerator = Util.determinant( _Lines[_Idx].Direction, _Lines[_LineNum].Point - _Lines[_Idx].Point )

        if math.fabs( _Denominator ) <= Define.RVO_EPSILON:
            if 0.0 > _Numerator:
                return False
            else:
                continue

        _t = _Numerator / _Denominator

        if _Denominator >= 0.0:
            _tRight = min( _tRight, _t )
        else:
            _tLeft = max( _tLeft, _t )

        if _tLeft > _tRight:
            return False

    _temp = FCVector.vector2D( 0, 0 )
    if _DirectionOpt:
        if 0.0 < _OptVelocity.dot( _Lines[_LineNum].Direction ):
            _temp = _Lines[_LineNum].Point + _tRight * _Lines[_LineNum].Direction
        else:
            _temp = _Lines[_LineNum].Point + _tLeft * _Lines[_LineNum].Direction
    else:
        _t = _Lines[_LineNum].Direction.dot(_OptVelocity - _Lines[_LineNum].Point)

        if _t < _tLeft:
            _temp = _Lines[_LineNum].Point + _Lines[_LineNum].Direction * _tLeft
        elif _t > _tRight:
            _temp = _Lines[_LineNum].Point + _Lines[_LineNum].Direction * _tRight
        else:
            _temp = _Lines[_LineNum].Point + _Lines[_LineNum].Direction * _t

    _Result.x = _temp.x
    _Result.y = _temp.y
    
    return True


def LinearProgram2( _Lines, _Radius, _OptVelocity, _DirectionOpt, _Result ):
    _result1 = FCVector.vector2D( 0, 0 )
    if _DirectionOpt:
        _result1 = _OptVelocity * _Radius;
    elif _OptVelocity.lengthsq() > Util.square( _Radius ):
        _result1 = FCVector.normalize(_OptVelocity) * _Radius
    else:
        _result1 = _OptVelocity

    _Result.x = _result1.x
    _Result.y = _result1.y

    _idx = 0
    for _line in _Lines:
        if 0.0 < Util.determinant( _line.Direction, _line.Point - _Result ):
            _TempResult = FCVector.vector2D( 0, 0 )
            _TempResult.x = _Result.x
            _TempResult.y = _Result.y
            if not LinearProgram1( _Lines, _idx, _Radius, _OptVelocity, _DirectionOpt, _Result ):
                _Result.x = _TempResult.x
                _Result.y = _TempResult.y
                return _idx
                
        _idx += 1

    return len(_Lines)

def LinearProgram3( _Lines, _NumObstLines, _BeginLine, _Radius, _Result ):
    _Distance = 0.0
    
    for _Idx1 in xrange( _BeginLine, len(_Lines) ):
        if Util.determinant(_Lines[_Idx1].Direction, _Lines[_Idx1].Point - _Result) > _Distance:
            _ProjectionLines = []
            _ProjectionLines.extend( _Lines[:_NumObstLines] )

            for _Idx2 in xrange( _NumObstLines, _Idx1 ):
                _Line = Define.Line()
                _Determinant = Util.determinant( _Lines[_Idx1].Direction, _Lines[_Idx2].Direction )

                if Define.RVO_EPSILON >= math.fabs( _Determinant ):
                    if 0.0 < _Lines[_Idx1].Direction.dot( _Lines[_Idx2].Direction ):
                        continue
                    else:
                        _Line.Point = (_Lines[_Idx1].Point + _Lines[_Idx2].Point) * 0.5
                else:
                    _Line.Point = _Lines[_Idx1].Point + ( Util.determinant( _Lines[_Idx2].Direction, _Lines[_Idx1].Point - _Lines[_Idx2].Point) / _Determinant ) * _Lines[_Idx1].Direction;

                _Line.Direction = FCVector.normalize(_Lines[_Idx2].Direction - _Lines[_Idx1].Direction)
                _ProjectionLines.append( _Line )

            _TempResult = FCVector.vector2D( 0, 0 )
            _TempResult.x = _Result.x
            _TempResult.y = _Result.y
            if LinearProgram2( _ProjectionLines, _Radius, FCVector.vector2D( -_Lines[_Idx1].Direction.y, _Lines[_Idx1].Direction.x ), True, _Result) < len(_ProjectionLines):
                _Result.x = _TempResult.x
                _Result.y = _TempResult.y

            _Distance = Util.determinant( _Lines[_Idx1].Direction, _Lines[_Idx1].Point - _Result )